#include "DispatcherHandler.h"
#include "PwmOut.h"
#include "SW1248.h"
#include "CmdLine.h"

#include "DataStorage.h"
#include "log.h"
#include "DataStorageStepper.h"
#include "EEPROM.h"
#include "linkage_steps_factory.h"

DispatcherHandler::DispatcherHandler(I2CSlave& i2c_slave,DataStorage& dataStorage,SMCEngine& smcEngine,SMC& smc):
    _i2c_slave(i2c_slave),
    _ds(dataStorage),
    _smcEngine(smcEngine),
    _smc(smc)
{

}

DispatcherHandler::~DispatcherHandler()
{
    
}
void DispatcherHandler::begin()
{
  
   module_general_reg_t& general_reg = DataStorageStepper::module_reg().module_general_reg;
   
   // 更新驱动器
    _smc.tb67s109a_electrical_control().device1.s0  = general_reg.module_general_info[0].current_limit_setup_reg.s0;
    _smc.tb67s109a_electrical_control().device1.s1  = general_reg.module_general_info[0].current_limit_setup_reg.s1;
    _smc.tb67s109a_electrical_control().device1.s2  = general_reg.module_general_info[0].current_limit_setup_reg.s2;
    _smc.tb67s109a_electrical_control().device1.m0  = general_reg.module_general_info[0].step_resolution_setup_reg.m0;
    _smc.tb67s109a_electrical_control().device1.m1  = general_reg.module_general_info[0].step_resolution_setup_reg.m1;
    _smc.tb67s109a_electrical_control().device1.m2  = general_reg.module_general_info[0].step_resolution_setup_reg.m2;
    _smc.tb67s109a_electrical_control().device1.en  = general_reg.control_enable_reg.m1;

    _smc.tb67s109a_electrical_control().device2.s0  = general_reg.module_general_info[1].current_limit_setup_reg.s0;
    _smc.tb67s109a_electrical_control().device2.s1  = general_reg.module_general_info[1].current_limit_setup_reg.s1;
    _smc.tb67s109a_electrical_control().device2.s2  = general_reg.module_general_info[1].current_limit_setup_reg.s2;
    _smc.tb67s109a_electrical_control().device2.m0  = general_reg.module_general_info[1].step_resolution_setup_reg.m0;
    _smc.tb67s109a_electrical_control().device2.m1  = general_reg.module_general_info[1].step_resolution_setup_reg.m1;
    _smc.tb67s109a_electrical_control().device2.m2  = general_reg.module_general_info[1].step_resolution_setup_reg.m2;
    _smc.tb67s109a_electrical_control().device2.en  = general_reg.control_enable_reg.m2;

    _smc.tb67s109a_electrical_control().device3.s0  = general_reg.module_general_info[2].current_limit_setup_reg.s0;
    _smc.tb67s109a_electrical_control().device3.s1  = general_reg.module_general_info[2].current_limit_setup_reg.s1;
    _smc.tb67s109a_electrical_control().device3.s2  = general_reg.module_general_info[2].current_limit_setup_reg.s2;
    _smc.tb67s109a_electrical_control().device3.m0  = general_reg.module_general_info[2].step_resolution_setup_reg.m0;
    _smc.tb67s109a_electrical_control().device3.m1  = general_reg.module_general_info[2].step_resolution_setup_reg.m1;
    _smc.tb67s109a_electrical_control().device3.m2  = general_reg.module_general_info[2].step_resolution_setup_reg.m2;
    _smc.tb67s109a_electrical_control().device3.en  = general_reg.control_enable_reg.m3;
    _smc.tb67s109a_electrical_control().device3.dir = 1;
    
    // log::debug("%x:%x:%x\r\n",_smc.tb67s109a_electrical_control().device1.value,_smc.tb67s109a_electrical_control().device2.value,_smc.tb67s109a_electrical_control().device3.value);
    //处理M1设置
    general_reg.master_update_data_reg.value= 0x07;
    this->update();
}

void DispatcherHandler::update()
{
    module_general_reg_t& general_reg = DataStorageStepper::module_reg().module_general_reg;
    //处理M1设置
    if(general_reg.master_update_data_reg.m1 == 1){
        //清空累计运行的总步数
        general_reg.master_update_data_reg.m1 =0;
        general_reg.module_general_info[0].total_motion_steps_stats_reg.value =0;
        _smc.mc(0).reset();
        //log::debug("0x42->M1\r\n");
    }
    
    //处理M2设置
    if(general_reg.master_update_data_reg.m2 == 1){
        //清空累计运行的总步数
        general_reg.master_update_data_reg.m2 =0;
        general_reg.module_general_info[1].total_motion_steps_stats_reg.value =0;
        //运动算法配置-加速度
        _smc.mc(1).reset();
        //log::debug("0x42->M2\r\n");
        
    }
    //处理M3设置
    if(general_reg.master_update_data_reg.m3 == 1){
        //清空累计运行的总步数
        general_reg.master_update_data_reg.m3 =0;
        general_reg.module_general_info[2].total_motion_steps_stats_reg.value =0;
        //运动算法配置-加速度
        _smc.mc(2).reset();
        //log::debug("0x42->M3\r\n");
    }

    //更新电气配置
    _smc.update_electrical_devices(); 
}
void DispatcherHandler::schedule()
{
    if(!_i2c_slave.read(_reg_data_read)){
        return;
    }
    module_general_reg_t& general_reg = DataStorageStepper::module_reg().module_general_reg;

    log::debug("mem:%x:%x:%x-%x:%x:%x\r\n",
                _reg_data_read.memory.data[0],
                _reg_data_read.memory.data[1],
                _reg_data_read.memory.data[2],
                _reg_data_read.memory.data[3],
                _reg_data_read.memory.data[4],
                _reg_data_read.memory.data[5]);

    switch(_reg_data_read.memory.data[0]){
        case 0x40://主机有新数据:
        {
            general_reg.master_update_data_reg.value = _reg_data_read.memory.data[1];
        }
        break;
        case 0x41://中断信号
        {
            general_reg.slave_interrupt_reg.value = _reg_data_read.memory.data[1];
            _smcEngine.clear_interrupt();
        }
        break;
        case 0x42://更新设置
        {
            this->update();
        }
        break;
        case 0x43://使能对应的驱动器
        {
            general_reg.control_enable_reg.value = _reg_data_read.memory.data[1];
            _smc.tb67s109a_electrical_control().device1.en = general_reg.control_enable_reg.m1;
            _smc.tb67s109a_electrical_control().device2.en = general_reg.control_enable_reg.m2;
            _smc.tb67s109a_electrical_control().device3.en = general_reg.control_enable_reg.m3;
            log::debug("0x43->595:%d->%x:%x:%x\r\n",_smc.tb67s109a_electrical_control().device1.en,
                                                    _smc.tb67s109a_electrical_control().data[0],
                                                    _smc.tb67s109a_electrical_control().data[1],
                                                    _smc.tb67s109a_electrical_control().data[2]);
            _smc.update_electrical_devices();
        }
        break;
        case 0x44://运动模式
        {
            general_reg.motion_mode_reg.value = _reg_data_read.memory.data[1];
        }
        break;
        case 0x45://保留
        break;
        case 0x46://M1电机运动步数累计总和-只读
        case 0x47:
        case 0x48:
        case 0x49:
        {
            // general_reg.module_general_info[0].total_motion_steps_stats_reg.steps_u16.low_8bit = _reg_data_read.memory.data[1];
            // general_reg.module_general_info[0].total_motion_steps_stats_reg.steps_u16.high_8bit= _reg_data_read.memory.data[2];
            // general_reg.module_general_info[0].total_motion_steps_stats_reg.delay_u16.low_8bit = _reg_data_read.memory.data[3];
            // general_reg.module_general_info[0].total_motion_steps_stats_reg.delay_u16.low_8bit = _reg_data_read.memory.data[4];
        }
        break;
        case 0x4A://M2电机运动步数累计总和-只读
        case 0x4B:
        case 0x4C:
        case 0x4D:
        {
            // general_reg.module_general_info[1].total_motion_steps_stats_reg.steps_u16.low_8bit = _reg_data_read.memory.data[1];
            // general_reg.module_general_info[1].total_motion_steps_stats_reg.steps_u16.high_8bit= _reg_data_read.memory.data[2];
            // general_reg.module_general_info[1].total_motion_steps_stats_reg.delay_u16.low_8bit = _reg_data_read.memory.data[3];
            // general_reg.module_general_info[1].total_motion_steps_stats_reg.delay_u16.low_8bit = _reg_data_read.memory.data[4];
        }
        break;
        case 0x4E://M3电机运动步数累计总和-只读
        case 0x4F:
        case 0x50:
        case 0x51:
        {
            // general_reg.module_general_info[2].total_motion_steps_stats_reg.steps_u16.low_8bit = _reg_data_read.memory.data[1];
            // general_reg.module_general_info[2].total_motion_steps_stats_reg.steps_u16.high_8bit= _reg_data_read.memory.data[2];
            // general_reg.module_general_info[2].total_motion_steps_stats_reg.delay_u16.low_8bit = _reg_data_read.memory.data[3];
            // general_reg.module_general_info[2].total_motion_steps_stats_reg.delay_u16.low_8bit = _reg_data_read.memory.data[4];
        }
        break;
        case 0x52://M1加减速
        case 0x53:
        {
            general_reg.module_general_info[0].acc_and_dec_setup_reg.low_8bit = _reg_data_read.memory.data[1];
            general_reg.module_general_info[0].acc_and_dec_setup_reg.high_8bit = _reg_data_read.memory.data[2];
             int16_t v  = general_reg.module_general_info[0].acc_and_dec_setup_reg.value;
            _smc.mc(0).setAccelerationInStepsPerSecondPerSecond(v);
        }
        break;
        case 0x54://M2加减速
        case 0x55:
        {
            general_reg.module_general_info[1].acc_and_dec_setup_reg.low_8bit = _reg_data_read.memory.data[1];
            general_reg.module_general_info[1].acc_and_dec_setup_reg.high_8bit = _reg_data_read.memory.data[2];
            int16_t v  = general_reg.module_general_info[1].acc_and_dec_setup_reg.value;
            _smc.mc(1).setAccelerationInStepsPerSecondPerSecond(v);
        }
        break;
        case 0x56://M3加减速
        case 0x57:
        {
            general_reg.module_general_info[2].acc_and_dec_setup_reg.low_8bit = _reg_data_read.memory.data[1];
            general_reg.module_general_info[2].acc_and_dec_setup_reg.high_8bit = _reg_data_read.memory.data[2];
            int16_t v  = general_reg.module_general_info[2].acc_and_dec_setup_reg.value;
            _smc.mc(2).setAccelerationInStepsPerSecondPerSecond(v);
        }
        break;
        case 0x58://保留
        break;
        case 0x59://累计总的M1联动步数统计--只读
        {
            //general_reg.module_general_info[0].total_linkage_steps_stats_reg.value = _reg_data_read.memory.data[1];
        }
        break;
        case 0x5A://累计总的M2联动步数统计--只读
        {
            //general_reg.module_general_info[1].total_linkage_steps_stats_reg.value = _reg_data_read.memory.data[1];
        }
        break;
        case 0x5B://累计总的M3联动步数统计--只读
        {
            //general_reg.module_general_info[2].total_linkage_steps_stats_reg.value = _reg_data_read.memory.data[1];
        }
        break;
        case 0x5C://M1联动循环次数
        case 0x5D:
        {
            general_reg.module_general_info[0].total_cycle_count_setup_reg.low_8bit = _reg_data_read.memory.data[1];
            general_reg.module_general_info[0].total_cycle_count_setup_reg.high_8bit = _reg_data_read.memory.data[2];
        }
        break;
        case 0x5E://M2联动循环次数
        case 0x5F:
        {
            general_reg.module_general_info[1].total_cycle_count_setup_reg.low_8bit = _reg_data_read.memory.data[1];
            general_reg.module_general_info[1].total_cycle_count_setup_reg.high_8bit = _reg_data_read.memory.data[2];
        }
        break;
        case 0x60://M3联动循环次数
        case 0x61:
        {
            general_reg.module_general_info[2].total_cycle_count_setup_reg.low_8bit = _reg_data_read.memory.data[1];
            general_reg.module_general_info[2].total_cycle_count_setup_reg.high_8bit = _reg_data_read.memory.data[2];
        }
        break;
        case 0x62://运动触发
        {
            general_reg.motion_trigger_reg.value = _reg_data_read.memory.data[1];

            if(general_reg.motion_trigger_reg.m1t==1){
                general_reg.motion_trigger_reg.m1t =0;
                //启动定时器1A
                if(general_reg.module_general_info[0].total_cycle_count_setup_reg.value == 0){
                    break;
                }

                if( general_reg.module_general_info[0].total_linkage_steps_stats_reg.value >0){
                    _smcEngine.m1_fire();
                }

            }
            if(general_reg.motion_trigger_reg.m2t==1){
                 general_reg.motion_trigger_reg.m2t =0;
                //启动定时器1B
                if(general_reg.module_general_info[1].total_cycle_count_setup_reg.value == 0){
                    break;
                }

                if( general_reg.module_general_info[1].total_linkage_steps_stats_reg.value >0){
                    _smcEngine.m2_fire();
                }
            }
            if(general_reg.motion_trigger_reg.m3t==1){
                 general_reg.motion_trigger_reg.m3t =0;
                //启动定时器2A
                if(general_reg.module_general_info[2].total_cycle_count_setup_reg.value == 0){
                    break;
                }

                if( general_reg.module_general_info[2].total_linkage_steps_stats_reg.value >0){
                    _smcEngine.m3_fire();
                }
            }
        }
        break;
        case 0x63://M1步进电机细分
        {
            general_reg.module_general_info[0].step_resolution_setup_reg.value = _reg_data_read.memory.data[1];
            
            _smc.tb67s109a_electrical_control().device1.m0 =  general_reg.module_general_info[0].step_resolution_setup_reg.m0;
            _smc.tb67s109a_electrical_control().device1.m1 =  general_reg.module_general_info[0].step_resolution_setup_reg.m1;
            _smc.tb67s109a_electrical_control().device1.m2 =  general_reg.module_general_info[0].step_resolution_setup_reg.m2;
            general_reg.master_update_data_reg.m1 = 1;
            _smc.update_electrical_devices();
        }
        break;
        case 0x64://M2步进电机细分
        {
            general_reg.module_general_info[1].step_resolution_setup_reg.value = _reg_data_read.memory.data[1];
            _smc.tb67s109a_electrical_control().device2.m0 = general_reg.module_general_info[1].step_resolution_setup_reg.m0;
            _smc.tb67s109a_electrical_control().device2.m1 = general_reg.module_general_info[1].step_resolution_setup_reg.m1;
            _smc.tb67s109a_electrical_control().device2.m2  = general_reg.module_general_info[1].step_resolution_setup_reg.m2;
            general_reg.master_update_data_reg.m2 = 1;
            _smc.update_electrical_devices();
        }
        break;
        case 0x65://M3步进电机细分
        {
            general_reg.module_general_info[2].step_resolution_setup_reg.value = _reg_data_read.memory.data[1];
            _smc.tb67s109a_electrical_control().device3.m0 = general_reg.module_general_info[2].step_resolution_setup_reg.m0;
            _smc.tb67s109a_electrical_control().device3.m1  = general_reg.module_general_info[2].step_resolution_setup_reg.m1;
            _smc.tb67s109a_electrical_control().device3.m2 = general_reg.module_general_info[2].step_resolution_setup_reg.m2;
            general_reg.master_update_data_reg.m3 = 1;
            _smc.update_electrical_devices();
        }
        break;
        case 0x66://M1电机限流
        {
            general_reg.module_general_info[0].current_limit_setup_reg.value = _reg_data_read.memory.data[1];
            _smc.tb67s109a_electrical_control().device1.s0 = general_reg.module_general_info[0].current_limit_setup_reg.s0;
            _smc.tb67s109a_electrical_control().device1.s1 = general_reg.module_general_info[0].current_limit_setup_reg.s1;
            _smc.tb67s109a_electrical_control().device1.s2 = general_reg.module_general_info[0].current_limit_setup_reg.s2;
            general_reg.master_update_data_reg.m1 = 1;
            _smc.update_electrical_devices();
        }
        break;
        case 0x67://M2电机限流
        {   
            general_reg.module_general_info[1].current_limit_setup_reg.value = _reg_data_read.memory.data[1];
            _smc.tb67s109a_electrical_control().device2.s0 = general_reg.module_general_info[1].current_limit_setup_reg.s0;
            _smc.tb67s109a_electrical_control().device2.s1 = general_reg.module_general_info[1].current_limit_setup_reg.s1;
            _smc.tb67s109a_electrical_control().device2.s2 = general_reg.module_general_info[1].current_limit_setup_reg.s2;
            general_reg.master_update_data_reg.m2 = 1;
            _smc.update_electrical_devices();
        }
        break;
        case 0x68://M3电机限流
        {
            general_reg.module_general_info[2].current_limit_setup_reg.value = _reg_data_read.memory.data[1];
            _smc.tb67s109a_electrical_control().device3.s0 = general_reg.module_general_info[2].current_limit_setup_reg.s0;
            _smc.tb67s109a_electrical_control().device3.s1 = general_reg.module_general_info[2].current_limit_setup_reg.s1;
            _smc.tb67s109a_electrical_control().device3.s2 = general_reg.module_general_info[2].current_limit_setup_reg.s2;
            general_reg.master_update_data_reg.m3 = 1;
            _smc.update_electrical_devices();
        }
        break;
        case 0xFA://添加M1联动数据
        {
            auto x = linkage_steps_factory::setup_allocator().alloc();
            if(x==nullptr){
                return;
            }
            x->memory.data[0] = _reg_data_read.memory.data[1];
            x->memory.data[1] = _reg_data_read.memory.data[2];
            x->memory.data[2] = _reg_data_read.memory.data[3];
            x->memory.data[3] = _reg_data_read.memory.data[4];
            
            linkage_steps_factory::linkage_steps_setups(0).push_back(x);
            general_reg.module_general_info[0].total_linkage_steps_stats_reg.value +=1;
            EEPROM.write(1000+(linkage_steps_factory::linkage_steps_setups(0).size()-1)*4,x->memory.data[0]);
            EEPROM.write(1000+(linkage_steps_factory::linkage_steps_setups(0).size()-1)*4+1,x->memory.data[1]);
            EEPROM.write(1000+(linkage_steps_factory::linkage_steps_setups(0).size()-1)*4+2,x->memory.data[2]);
            EEPROM.write(1000+(linkage_steps_factory::linkage_steps_setups(0).size()-1)*4+3,x->memory.data[3]);
        }
        break;
        case 0xFB://添加M2联动数据
        {
            auto x = linkage_steps_factory::setup_allocator().alloc();
            if(x==nullptr){
                return;
            }
            x->memory.data[0] = _reg_data_read.memory.data[1];
            x->memory.data[1] = _reg_data_read.memory.data[2];
            x->memory.data[2] = _reg_data_read.memory.data[3];
            x->memory.data[3] = _reg_data_read.memory.data[4];
            linkage_steps_factory::linkage_steps_setups(1).push_back(x);
            general_reg.module_general_info[1].total_linkage_steps_stats_reg.value +=1;
            EEPROM.write(1200+(linkage_steps_factory::linkage_steps_setups(1).size()-1)*4,x->memory.data[0]);
            EEPROM.write(1200+(linkage_steps_factory::linkage_steps_setups(1).size()-1)*4+1,x->memory.data[1]);
            EEPROM.write(1200+(linkage_steps_factory::linkage_steps_setups(1).size()-1)*4+2,x->memory.data[2]);
            EEPROM.write(1200+(linkage_steps_factory::linkage_steps_setups(1).size()-1)*4+3,x->memory.data[3]);
        }
        break;
        case 0xFC://添加M3联动数据
        {
            auto x = linkage_steps_factory::setup_allocator().alloc();
            if(x==nullptr){
                return;
            }
            x->memory.data[0] = _reg_data_read.memory.data[1];
            x->memory.data[1] = _reg_data_read.memory.data[2];
            x->memory.data[2] = _reg_data_read.memory.data[3];
            x->memory.data[3] = _reg_data_read.memory.data[4];
            linkage_steps_factory::linkage_steps_setups(2).push_back(x);
            general_reg.module_general_info[2].total_linkage_steps_stats_reg.value +=1;
            EEPROM.write(1400+(linkage_steps_factory::linkage_steps_setups(2).size()-1)*4,x->memory.data[0]);
            EEPROM.write(1400+(linkage_steps_factory::linkage_steps_setups(2).size()-1)*4+1,x->memory.data[1]);
            EEPROM.write(1400+(linkage_steps_factory::linkage_steps_setups(2).size()-1)*4+2,x->memory.data[2]);
            EEPROM.write(1400+(linkage_steps_factory::linkage_steps_setups(2).size()-1)*4+3,x->memory.data[3]);
        }
        break;
        case 0xFD://写M1联动配置
        {
            linkage_steps_factory::clear(0);
            general_reg.module_general_info[0].total_linkage_steps_stats_reg.value = 0;
        }
        break;
        case 0xFE://写M2联动配置
        {
            linkage_steps_factory::clear(1);
            general_reg.module_general_info[1].total_linkage_steps_stats_reg.value = 0;
        }
        break;
        case 0xFF://写M2联动配置
        {
            linkage_steps_factory::clear(2);
            general_reg.module_general_info[2].total_linkage_steps_stats_reg.value = 0;
        }
        break;
        default:
        break;
    }
   
}
          
void DispatcherHandler::cleanup()
{

}


data_storage_reg_read_t& DispatcherHandler::get_reg_data(uint8_t reg_address)
{
    module_general_reg_t& module_general_reg = DataStorageStepper::module_reg().module_general_reg;
    switch(reg_address)
    {
        case 0x41:
        {
            static uint8_t interrupt_data[1];
            interrupt_data[0] = module_general_reg.control_enable_reg.value;
            _reg_read.data    =  interrupt_data;
            _reg_read.size    = 1;
            //清除中断
            _smcEngine.clear_interrupt();
        }
        break;
        case 0xFA://读M1联动配置
        {
            //uint8_t v = module_general_reg.module_general_info[0].total_linkage_steps_stats_reg.value;
            auto x = linkage_steps_factory::linkage_steps_setups(0).peek_front();

            while(x){
                _i2c_slave.write(x->memory.data,4);
                x = x->next;
            } 
            
            if(module_general_reg.module_general_info[0].total_linkage_steps_stats_reg.value!= EEPROM.read(40+18))
            {
                EEPROM.write(40+18,module_general_reg.module_general_info[0].total_linkage_steps_stats_reg.value);
            }
            // Serial.println(EEPROM.read(40+18));
            return _reg_read;
        }   
        break;
        case 0xFB://读M2联动配置
        {
            //uint8_t v = module_general_reg.module_general_info[1].total_linkage_steps_stats_reg.value;
            auto x = linkage_steps_factory::linkage_steps_setups(1).peek_front();
            while(x){
                _i2c_slave.write(x->memory.data,4);
                x = x->next;
            } 
            // Serial.println(EEPROM.read(40+33));
            if(module_general_reg.module_general_info[1].total_linkage_steps_stats_reg.value!= EEPROM.read(40+33))
            {
                EEPROM.write(40+33,module_general_reg.module_general_info[1].total_linkage_steps_stats_reg.value);
            }
            return _reg_read;
        }
        break;
        case 0xFC://读M3联动配置
        {
            //uint8_t v = module_general_reg.module_general_info[2].total_linkage_steps_stats_reg.value;
            auto x = linkage_steps_factory::linkage_steps_setups(2).peek_front();
            while(x){
                _i2c_slave.write(x->memory.data,4);
                x = x->next;
            } 
            // Serial.println(EEPROM.read(40+48));
            if(module_general_reg.module_general_info[2].total_linkage_steps_stats_reg.value!= EEPROM.read(40+48))
            {
                EEPROM.write(40+48,module_general_reg.module_general_info[2].total_linkage_steps_stats_reg.value);
            }
            return _reg_read;
        }
        break;
        default:
        {
           data_storage_reg_read_t & data = _ds.get_reg_data(reg_address);
            _reg_read.data  = data.data;
            _reg_read.size  = data.size;
        }
        break;
    }
    
    _i2c_slave.write((const uint8_t *)_reg_read.data,_reg_read.size);
    return  _reg_read;
}